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Abstract 

Determining  the  position  of  team  member’s  is  always  useful  information,  whether 
it  is  a  team  of  fire  fighters  fighting  a  blaze  or  combatants  clearing  a  building  in  the 
field.  This  information  becomes  even  more  decisive  for  the  people  responsible  for 
their  safety.  To  accomplish  this  in  areas  denied  Global  Navigation  Satellite  System 
(GNSS),  such  as  around  buildings  or  in  steep  valleys,  alternative  methods  must  be 
used.  Radio  ranging  systems  have  been  a  part  of  the  navigation  solution  for  years. 
They  unfortunately  have  poor  performance  in  certain  areas,  such  as  inside  buildings, 
due  to  multipath  and  other  errors.  To  improve  the  position  estimate  it  is  believed 
that  using  vision,  consumer  grade  inertial  navigation  systems,  and  any  other  mea¬ 
surement  available  can  aid  the  navigation  solution.  To  accomplish  this,  an  extended 
Kalman  filter  was  developed.  It  was  configured  as  a  centralized  filter.  This  produced 
a  baseline,  showing  that  as  image  measurements  were  added,  the  navigation  solution 
did  improve.  To  simulate  this  with  multiple  vehicles  and/or  soldiers  required  a  large 
state  vector  for  the  Kalman  filter.  To  manage  the  large  number  of  states  and  effi¬ 
ciently  incorporate  them  into  influence  matrices,  a  "Rosetta  stone"  was  designed  for 
state  management.  This  "Rosetta  stone"  breaks  the  states  into  simpler  blocks  such 
as  position  and  attitude  for  the  soldier  and  position  for  the  image  features.  This  in 
turn  made  updating  the  influence  matrix  and  covariance  matrix  a  smoother  process. 
The  impact  of  adding  image  measurements  has  been  two  fold.  First,  the  position 
RMS  errors  were  reduced  by  approximately  a  factor  of  2.  Second,  the  attitude  which 
fluctuated  greatly  in  the  radio  only  cases  was  reduced  by  a  factor  of  10  through 
image  aiding. 
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Performance  Enhancements  of  Ranging  Radio  Aided  Navigation 


I.  Introduction 

This  document’s  purpose  is  to  describe  the  research  in  the  application  of  multi 
vehicle  radio  ranging  and  the  benefit  of  adding  imaging  to  its  navigation  solution.  The 
need  to  ensure  precision  navigation  in  locations  where  Global  Navigation  Satellite 
Systems  (GNSS)  do  not  work  has  become  increasingly  valuable.  Since  the  1991  Gulf 
War,  precision  navigation  has  been  crucial  to  miliary  actions  [8].  This  research  is 
one  facet  of  the  research  being  accomplished  to  reduce  the  reliance  placed  on  GNSS. 
The  use  of  radio  ranging  in  positioning  has  been  used  for  years  [7] .  It  however  has 
limitations  when  used  as  a  stand-alone  solution.  Starting  with  geometry,  generally 
in  a  scenario  were  soldiers  are  maneuvering  on  the  ground,  there  is  rarely  significant 
differences  in  height  of  the  soldiers  thus  reducing  the  accuracy  of  the  height  solution. 
Radio  systems  used  in  urban  environment  also  have  difficulties  with  multipath  as  the 
soldiers  move  through  the  building.  There  can  also  be  significant  clock  errors  from 
the  need  to  keep  all  the  radios  synchronized  in  time.  These  errors  can  be  reduced 
with  the  aid  of  vision  measurements  and  inertial  measurements. 

1.1  Problem  Statement 

The  goal  of  this  research  is  to  develop  the  ability  to  simulate  multiple  vehicles 
that  can  incorporate  combinations  of  different  measurements  to  specifically:  radio 
inter-vehicle  ranging,  fixed  radio  ranging,  imaging,  and  barometer-altimeter  mea¬ 
surements.  The  primary  objective  is  to  determine  the  overall  performance  impacts 
of  the  different  measurements  together,  with  particular  emphasis  on  the  benefits  of 
adding  vision/inertial  measurements  to  a  radio  ranging  based  system. 
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1.2  Assumptions 

The  first  assumption  is  that  the  image  ranging  will  be  accomplished  using  a 
single  camera.  One  approach  for  this  is  using  coded  aperture  which  is  currently  being 
developed  at  the  Air  Force  Institute  of  Technology  (AFIT)  [3]  and  for  the  purposes 
of  this  research  will  be  assumed  to  be  viable.  This  scenario  also  assumes  that  there 
is  no  limit  on  the  communications  bandwidth  for  the  radio.  This  allows  unlimited 
amounts  of  data  to  be  transmitted  between  soldiers  supporting  later  assumptions.  It 
is  also  assumed  that  the  there  is  no  measurement  delay  which  would  not  be  true  for 
live  systems.  It  is  also  assumed  that  the  measurements  are  available  at  the  central 
processing  mode  so  all  the  measurements  are  available  for  the  centralized  extended 
Kalman  filter. 

1.3  Methodology 

To  document  the  research  conducted  in  this  thesis,  each  chapter  will  be  briefly 
summarized.  The  first  chapter  is  an  overview  of  research  conducted  specifically 
in  the  area  of  incorporating  multi-vehicle  with  multiple  measurement  types.  The 
second  chapter  is  the  background  for  the  research.  This  includes  basic  navigation 
overview  to  include  frames  of  reference  and  the  methods  to  go  between  frames.  A 
brief  synopsis  of  imaging  is  included.  Kalman  filtering  and  extended  Kalman  filtering 
is  briefly  examined.  Chapter  three  is  work  unique  to  this  research.  This  begins  with 
the  design  of  the  extended  Kalman  filter  used  in  the  research  and  the  means  of 
incorporating  multiple  vehicles.  Chapter  four  consists  of  simulations  and  the  results 
will  describe  the  different  test  scenarios  and  discuss  the  results  from  them.  Finally, 
chapter  five  will  be  the  conclusion  and  recommendations.  It  will  be  a  synopsis  of  the 
results  and  other  work  that  can  be  done  to  move  this  research  forward. 
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II.  Navigation  Mathematics  Background 

2.1  Coordinate  Frames 

The  World  Geodetic  System  1984  (WGS  84)  is  a  common  three  dimensional 
coordinate  system.  The  system’s  defining  properties  is  that  it  is  geocentric  (center 
of  mass  is  defined  for  the  Earth).  The  orientation  was  initially  given  by  the  Bu¬ 
reau  International  de  l’Heure  (BIH)  of  1984.  WGS  84  is  a  right-handed  Earth-fixed 
orthogonal  system  all  of  which  is  fully  described  in  [6] . 

To  accomplish  navigation,  reference  frames  must  be  defined  in  order  to  deter¬ 
mine  position.  To  define  reference  frames  in  this  paper  [7]  and  [8]  were  used. 

•  Earth-fixed  inertial  frame  (i-frame)  Figure  2.1:  origin  is  defined  at  the  earth’s 
center  of  mass,  with  axes  that  are  non-rotating  with  respect  to  fixed  stars  (i.e., 
first  star  of  Aries). 

•  Earth-centered  Earth-fixed  frame  (e-frame)  Figure  2.1:  origin  is  at  the  Earth’s 
center  of  mass,  with  axes  fixed  to  and  rotating,  with  the  earth.  Its  axes  are 
z  along  the  Earth’s  polar  axis  through  the  north  pole,  x  is  in  the  equatorial 
plane  through  Greenwich  meridian,  and  y  is  also  on  the  equatorial  plane  90 
degrees  east  in  longitude  from  x. 

•  Vehicle  fixed  navigation  frame  (n’-frame)  Figure  2.1:  origin  is  at  the  center  of 
gravity  of  the  vehicle  or  another  point  on  the  vehicle.  The  axes  are  north, 
east  and  down  (NED)  from  the  origin. 

•  Earth  fixed  navigation  frame  (n-frame)  Figure  2.1:  has  an  origin  at  some  pre¬ 
defined  location  on  Earth,  and  its  axes  x,  y,  and  z  are  in  the  NED  direction 
as  in  the  vehicle  fixed  navigation  frame. 

•  Body  frame  (b- frame):  origin  is  the  same  as  the  vehicle  fixed  navigation  frame, 
and  typically  its  axes  x,  y ,  and  z  are  out  the  nose,  right  wing,  and  bottom  of 
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Figure  2.1:  Earth  with  the  i-frame,  e- frame,  and  n’-frames  [8] 


an  aircraft.  For  this  research,  the  axis  x,  y.  and  z  axes  are  out  the  nose  of  the 
troop,  the  right  shoulder,  and  down  through  the  soldier’s  body. 

•  Camera  frame  (c- frame):  origin  is  at  the  camera’s  optical  center.  The  camera 
frame  x  and  y  are  up  and  to  the  right  of  the  optical  center.  The  z  axes  is  out 
of  the  plane  defined  by  x  and  y  along  the  direction  the  camera  is  pointed. 


2.2  Coordinate  Frames  Transforms 

The  ability  to  move  between  different  coordinate  reference  frames  is  extremely 
important  in  navigation.  Using  [7]  and  [8]  the  relevant  transforms  will  be  developed. 
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Coordinate  frame  transforms  allow  vectors  to  be  converted  from  one  frame  of  refer¬ 
ence  to  another,  and  this  is  normally  accomplished  using  a  Direction  Cosine  Matrix 
(DCM).  DCM  is  expressed  as  a  3  x  3  matrix  which  is  the  inner  product  of  the  unit 
basis  vector  in  one  frame  with  each  of  the  unit  basis  vectors  in  another  frame. 

The  elements  of  the  DCM  are  broken  into  the  ith  row  and  jth  column  each 
representing  the  cosine  of  the  /-axis  of  the  a-frame  and  the  j-axis  of  the  b-frame. 

Cll  C12  C13 

C21  C22  C23  (2.1) 

C31  C32  C33 

pb  =  C  y  (2.2) 

The  DCM  has  the  following  properties  if  used  with  a  right  hand  Cartesian 
frame: 

1.  Det( Cj)  =  |CJ|  =  1 

2.  C‘  =  (C*)^  =  (Ci)T 

3.  C‘  =  CJCJ 

The  Earth  frame  to  navigation  frame  (north,  east,  down)  transformation  is 

sin  L  cos  A  —  sin  L  sin  A  cos  L 
—  sin  A  cos  A  0  (2.3) 

cos  L  cos  A  —  cos  L  sin  A  —  sin  A 

Where  L  is  the  latitude  and  A  longitude  the  navigation  frame  to  the  body 
frame  DCM  is  given  by  Equation  (2.4). 
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sin  ip  cos  9 


cos  ip  cos  9 

—  sin  ip  cos  <p  +  cos  ip  sin  6  sin  <p> 
sin  ip  sin  (p  +  cos  ip  sin  9  cos  <p> 


cos  ip  cos  (p  +  sin  ip  sin  9  sin  (p 
—  cos  ip  sin  (p  +  sin  ip  sin  9  cos  (p 


—  sin  9 

cos  9  sin  (p 

cos  9  cos  <p 
(2.4) 


Where  the  Euler  angles  are  represented  by  <p  as  roll,  9  as  pitch,  and  ip  as  yaw. 


2.3  Basic  Inertial  Navigation 

This  section  closely  follows  a  similar  development  given  in  [7].  To  perform 
inertial  navigation  requires  a  determination  of  angular  motion  through  gyroscopic 
sensors,  force  measurements  using  accelerometers,  the  ability  to  determine  force 
measurement  into  the  reference  frame  using  the  attitude  information  given  by  the 
gyroscopes,  the  ability  to  resolve  the  Earth’s  gravitational  force,  and  the  ability  to 
integrate  the  aforementioned  data  to  find  velocities  and  position. 

Gyroscopes  can  be  used  to  find  the  angle  turned  or  the  angular  rate  about  an 
axis.  There  are  many  types  of  gyroscopes  from  the  common  spinning  mass  gyroscopes 
to  the  gas  rate  gyroscope  [7].  The  spinning  mass  gyroscope  reacts  to  a  tilt  by 
moving  orthogonally  to  the  direction  of  the  tilt  and  then  allows  angle  moved  to  be 
measured.  There  are  three  reasons  a  gyroscope  works:  inertia,  angular  momentum, 
and  precession.  Inertia  is  important  because  it  defines  the  direction  that  is  fixed  in 
the  inertial  reference  frame.  Angular  momentum  is  the  product  of  the  inertia  and 
the  angular  velocity  about  the  same  axis  of  rotation. 

Angular  momentum  is  defined  as  the  ability  to  continue  to  rotate  about  a 
point  unless  acted  upon  by  an  external  torque.  It  is  important  to  consider  the 
angular  momentum  of  the  gyroscope  as  this  will  have  a  great  effect  on  the  drift  that 
the  gyroscope  will  experience.  Precession  is  important  in  the  accurate  measurement 
of  angular  rotation. 
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In  this  research  micro-machined  electromechanical  system  (MEMS)  sensors  are 
used.  MEMS  gyroscopes  use  Coriolis  acceleration  interaction  on  proof  masses.  These 
rely  on  the  force  interacting  with  the  mass  causing  vibrations  in  the  unit’s  reference 
frame.  The  Coriolis  force  and  linear  motion  force  result  in  a  force  perpendicular  to 
both  allowing  the  rotation  to  be  measured  [7]. 

Accelerometers  are  the  next  important  component  in  inertial  navigation;  how¬ 
ever,  it  must  be  pointed  out  that  accelerometers  measure  specific  force,  which  is  the 
combination  of  acceleration  and  gravity  effects. 

An  accelerometer  can  be  simply  modeled  as  a  known  mass  attached  to  a  spring 
allowing  force  to  be  determined  on  the  axes  of  the  accelerometer.  When  operating 
around  the  Earth  gravity  must  also  be  accounted  for. 

The  only  way  to  account  for  gravity  is  to  use  a  model  like  WGS  84  that  has 
an  approximation  for  gravity  everywhere  on  Earth.  With  this  data  gravity  can  be 
properly  removed  from  the  accelerometer. 

MEMS  accelerometers  are  put  into  two  groups  [7]: 

•  One  with  a  proof  mass  on  a  hinge  with  a  mechanical  sensor  to  detect  the 

applied  acceleration. 

•  The  other  uses  a  vibrating  element  that  is  measured  for  change  in  frequency 

of  the  element  as  tension  changes  with  the  application  of  force. 

The  information  from  the  accelerometer  then  needs  to  be  resolved  into  the 
proper  frame  of  reference.  This  is  accomplished  by  rotating  the  measurement  from 
the  body  frame  to  the  navigation  frame  using  a  direction  cosine  matrix  (DCM)  which 
is  continuously  updated  by  the  gyroscope  measurements. 
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2-4  Error  Models 

The  error  model  for  physical  was  developed  using  [5]  and  [8].  This  model  is 
based  on  a  Taylor  series  approximation  of  the  nominal  solution  in  the  nonlinear 
differential  equation. 


X  =  f[x(i),u(t),t] 

With  x(£)  representing  the  system  state  and  u(f)  the  control  input. 
Perturbation  about  the  nominal  can  be  expressed  as 


(2.5) 


X  =  f [xo(t),  Uo(t),  t]  +  F (t){x(t)  -  Xo(t)}  +  B(t){u(t)  -  u0(t)}  H -  (2.6) 


m 


5x|x0(t),u0(t),t 


m 


Qu  |x0(i),uo(b,t 


dh 

dxi  dxn 


8fn  .  .  .  8fn 

dxi  dxn 


Oh  dfi 

dui  dur 


df„  _  _  _  dfn 

dui  dur 


xo(*)>uo(*).* 


xO(t)'uo(*).t 


(2.7) 


(2.8) 


Neglecting  higher  order  terms,  an  approximation  of  the  differential  equation  or 
the  linearized  perturbation  equation  can  then  be  found  as 


x  =  Fx  +  Bu 


(2.9) 


Uncertainty  can  be  added  to  make  this  a  stochastic  differential  equation 


2-6 


X  =  Fx  +  Bu  +  Gw 


(2.10) 


Q  =  E[w(t)wT  (t  +  t)}5(t)  (2.11) 

The  solution  to  this  differential  equation  is  then  represented  as  [5] 

x  =  <E>  (t,  t0)  x  (t0)  +  /  <I>  (t,  t)  B  (t)  u  (t)  dr  +  f  <1>  (t,  r)  G  (t)  w  (t)  dr  (2.12) 

Jt  0  Jto 

Where  $  (t,  t0 )  is  the  state  transition  matrix  from  t0  to  t. 

2.5  Inertial  error  model 

The  inertial  navigation  system  navigation  error  model  will  be  created  based 
on  [5],  [8],  and  [2], 

To  begin  with,  the  inertial  senor  will  be  modeled.  The  accelerometers  and 
gyroscopes  are  modeled  using  a  first  order  Gauss-Markov  process  and  the  random 
noise  is  modeled  as  a  white  Gaussian  process  [8].  The  accelerometer  is  modeled  as 

fm  —  f6+a6+wa  (2-13) 

Where  fb  is  the  true  specific  force,  ab  is  the  accelerometer  bias  and  w)  is  the  white 
Gaussian  noise  (all  in  the  body  frame). 

In  a  similar  manner,  the  gyroscope  is  modeled  as 

uobm  =  LJb+hb+wbb  (2.14) 

Where  uob  is  the  true  angular  rate,  b/'  is  the  gyroscope  bias,  and  w['  is  the  white 
Gaussian  noise  (all  in  the  body  frame). 

The  first  order  Gauss-Markov  terms  ab  and  b,‘  are  modeled  as 
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(2.15) 


ab  = - a6+w 

Tn. 


Q’bia 


b‘=-ib‘+w^.  (2.16) 

1  0 

Where  ra  is  the  time  constant  for  the  accelerometer  and  T/,  is  the  time  constant 
of  the  gyroscope  and  w b  and  w?  represent  the  white  Gaussian  driving  noise  for 
the  accelerometer  and  gyroscope. 

Next,  the  attitude  errors  are  modeled  in  the  navigation  frame  about  the  NED 
frame. 

0  =  [en  ee  ed]T  where  en>  ee,  and  ed  represent  the  (small)  angular  attitude 
errors  about  the  NED  axis.  The  dynamics  model  of  these  angular  errors  is  given 
as  [8] 

=  -  [(CM.)  x]  Il>  -  CJb1  -  CJw?  (2.17) 

where  u>fe  is  the  earths  sidereal  angular  rate. 

Next  the  position  and  velocity  errors  are  represented  as 


5pn  =  pn  -  p" 

(2.18) 

5vn  =  vn  -  vn 

(2.19) 

where  pn  is  the  estimated  position  vector  and  pn  is  the  true  position  vector. 
The  velocity  error  dynamics  are  described  as  [8] 


=  CJGC'ip”  -  2c;«'ecyv  +  (f"x)  +  CJa6  +  CJwJ 


(2.20) 
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where  G  represents  the  gradient  of  the  gravity  vector  [8],  and  f2fe  is  a  skew-symmetric 
matrix  representing  the  Earth’s  rotation  rate  coordinatized  in  the  e-frame. 

5pn=  Svn  (2.21) 


2. 6  Imaging 

An  optical  sensor  is  used  to  collect  light  (possibly  other  frequencies)  and  deter¬ 
mine  the  strengths  [8].  An  optical  sensor  is  capable  of  creating  a  multi  dimensional 
measurement. 

Digital  optical  sensors  are  made  up  of  the  following  components:  lens,  aperture, 
and  sensor.  The  lens  focuses  the  light  on  the  sensor  and  the  amount  of  radiation  is 
controlled  by  the  aperture.  This  focused  light  creates  the  image.  The  image  is  then 
sampled  or  digitalized  by  the  sensor. 

A  camera  can  be  modeled  as  a  pin  hole  where  all  light  must  pass  through  the 
origin  and  is  inverted.  The  image  must  now  be  represented  in  terms  of  pixels  with 
the  origin  in  the  upper  left  hand  side,  as  seen  in  Figure  2.2. 

The  translation  between  the  vector  from  the  camera  to  the  feature  of  interest 
(sc)  and  pixel  coordinates  (sp*x)  is  accomplished  by  [8]. 


sPix  —  _ 


-M  o 

H  U 

0  A 
u  w 

0  0 


M+l 

2 

N+ 1 
2 

1 


or 


gPlX  _  rj-ipix  „C 


(2.22) 


(2.23) 
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Figure  2.2:  This  Figure  is  the  camera  pixel  description  from  [8] 
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t  Zn 


Figure  2.3:  This  figure  shows  the  difference  between  the  camera  and  the  soldier’s 
position  in  the  n- frame  [8]. 
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The  target  location  Figure  2.3  in  n-frame  can  be  obtained  and  the  pixel  coor¬ 


dinates  can  be  found  using  the  navigation  state. 


Sn  =  tn  -  pn 

(2.24) 

sc  =  CcbCbnsn 

(2.25) 

In  the  application  of  real  lenses  there  are  many  issues  that  arise  and  must  be 
accounted  for  to  eliminate  unwanted  errors.  For  example,  optical  distortions  are 
curvatures  of  straight  lines  at  the  edges  of  the  image  that  can  be  accounted  for  by 
reducing  issues  with  features  on  the  edge  of  the  field  of  view. 

2. 7  Kalman  filter 

2.7.1  Linear  Kalman  filter.  Using  [5],  [4],  and  [8]  the  Linear  Kalman  filter 
(KF)  will  now  be  developed.  The  linear  Kalman  filter  is  based  upon  a  linear  system 
driven  by  white  noise  with  deterministic  inputs. 

x  =  Fx  +  Bu  +  Gw  (2.26) 

Where  x  is  the  state  vector,  F  is  the  system  dynamics  matrix,  B  is  the  input  matrix 
u  is  the  input  vector,  G  is  the  noise  matrix,  and  w  is  the  white  noise  vector  with 
noise  strength  of 

Q(t)  =  E  [w(f)w7  (t  +  r)]  S(t)  (2.27) 

The  discrete-time  measurement  is 

z  (ti)  =  H (ti)x(ti)  +  v(ti)  (2.28) 

Where  v  (ti)  is  zero  mean  white  Gaussian  noise  described  by  the  covariance  matrix. 


2-12 


R  (U)  =  E  [v  (ti)  vr  (ti)]  (2.29) 

The  KF  measurement  update  incorporates  the  measurement  by 

K  (ti)  =  P(t-)B.T(U)  [H(t*)P  (t~)  nT(U)  +  R  (ti)] _1  (2.30) 

x(t+ )  =  x(tr )  +  K (U)  [z  ( U )  -  H \k(ti  )]  (2.31) 

P (tt)  =  P (ti)  +  K(tj)H(t)P  (tr)  (2.32) 

The  state  estimate  and  covariance  are  now  propagated  forward  in  time  by 

x(t/tj_  i)  =  F(t)x(t/ti_i)  +  B(t)n(t)  (2.33) 

where  (t/U- 1)  is  for  any  t  e  [ti-i,ti)  [5]. 

P (t/ti-r)  =  F(t)P(t/ti_1)  +  P(t/tj_1)FT(t)G(t)Q(t)GT  (t)  (2.34) 


2.1.2  Extended  Kalman  filter.  The  real  world  unfortunately  is  often  not 
linear,  and  as  such  requires  a  nonlinear  method  or  linearized  Kalman  filter,  such  as 
the  Extended  Kalman  Filter  (EKF).  Using  [4]  the  EKF  will  be  developed.  First  the 
systems  must  be  defined  by  its  now  nonlinear  dynamics  model. 

x(f)  =  f  [x  (t) ,  u  (t) ,  t]  +  G  (t)  w  (t)  (2.35) 

The  nonlinear  discrete  time  measurement  is 
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Z  (U)  =  hx(tj)  +  v(ti) 


(2.36) 


Where  v  (ti)  is  zero  mean  white  Gaussian  noise  with  strength  R (ti). 

The  EKF  measurement  update  incorporates  the  measurement  by 

K  (t4)  =  P((-)HT  [ti;  x  («-)]  {H  [ii; x  («“)]  P  («,")  HT  [tf, x  ((“)]  +  R  (*.") 

(2.37) 


x(t+)  =  x(i-  )  +  K(^)  {z  (ti)  -  h  [x(t-  ),ti]  }  (2.38) 

P(0  =  P(t-)  -  K(ti)H  P  (tr)  (2.39) 

Where  H  [tf,  *(*r)]  is  the  partial  derivative  matrix  which  linearizes  h  about 
the  nominal  trajectory: 

H  [<<;*«,-)]  =^^lx=S(«/«,)  (2.40) 

As  before,  the  estimate  is  propagated  forward  by 

Z(t/ti)  =  f  {H(t/ti),  u (t),t\  (2.41) 

P (t/U)  =  F  [t;  Z(t/U)]  P (t/U)  +  P(t/U)FT  [t;  Z(t/U)]  +  G(t)Q(t)GT  ( t )  (2.42) 
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Where  F  [t;  x(f/i,)|  is  the  partial  derivative  matrix  which  linearizes  f  about  the 
nominal  trajectory. 


F  [t-,Z{ti)]  = 


df  [x,  u  (t) ,  t] 

<9x 


x=x(£/ti) 


(2.43) 


This  chapter  has  reviewed  briefly  a  wide  variety  of  important  aspects  of  navi¬ 
gation.  From  MEMS  to  the  Kalman  equations.  All  of  these  concepts  will  be  put  to 
use  in  Chapter  3  as  the  multi  vehicle  filter  is  developed. 
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III.  Ranging  and  Image  Multi-  Vehicle  Navigation  algorithms 

This  chapter  will  be  used  to  show  how  the  centralized  Kalman  filter  was  developed 
to  produce  the  optimal  estimate  of  the  current  navigation  state  for  multiple  vehicles. 

First,  the  variables  will  be  broken  down  and  the  method  that  was  used  to 
track  all  the  variables  will  be  explained.  This  is  important  because  tracking  multiple 
vehicle  requires  clear  nomenclature  to  ensure  that  all  variables  are  tracked  and  not 
misused.  Then,  the  extended  Kalman  filter  used  will  be  developed. 

3. 1  The  "Rosetta  Stone  " 

To  keep  track  of  the  navigation  states  for  each  of  the  individuals  or  vehicles  the 
following  navigation  state  descriptor  was  created  for  each  vehicle.  First  the  Pinson 
states  were  addressed  by  assigning  an  index  of  each  vehicle’s  position,  velocity  and 
attitude.  Then  if  additional  states  are  needed,  such  as  accelerometer  bias,  they  are 
also  included  into  the  vehicle’s  descriptor.  Once  all  the  additional  states  are  added, 
the  landmark  states  are  added  and  the  descriptors  are  included.  These  descriptors 
are  used  throughout  the  code  to  identified  the  state  locations  in  a  succinct  manner. 
This  means  that  at  any  point  in  the  code,  it  is  easy  to  determine  the  state  index  for 
any  particular  error  type,  vehicle  or  landmark. 

3.2  State  Model 

In  this  section,  the  individual  soldier’s  states  and  noises  are  defined.  For  clari¬ 
fication,  vehicles  are  synonymous  with  soldiers.  First,  the  individual  variable  will  be 
defined.  <5P  is  the  initial  position  error  vector  with  components  8x ,  8y,  and  8z  rep¬ 
resenting  the  errors  in  the  n-frame  (NED)  respectively.  Wp  represents  the  dynamic 
driving  noise  with  NED  components  wx,  wy,  and  wz.  The  following  states  are  for 
each  individual  vehicle. 
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5  P  = 


Sx  Sy  Sz 


WP  = 


w7 


Wy  WZ 


Next  5V  is  the  initial  velocity  error  vector  with  components  Sx,  Sy,  and  Sz 
representing  the  errors  in  the  NED  respectively.  Wy  represents  the  dynamic  driving 
noise  of  the  velocity  and  wx,  wy,  and  wz. 


SY  = 


Sx  Sy  Sz 


Wv  = 


l  T 


W7 


Wn\ 


Wi 


Se  is  the  initial  attitude  error  vector  with  components  Sex,  Sey,  and  Sez  repre¬ 
senting  the  errors  in  the  NED  respectively.  W£  represents  the  dynamic  driving  noise 
of  the  attitude  and  w£X,  w£y ,  and  w£Z. 


r  -| 

T 

r  i 

Ssx 

w£  = 

^ex  ^ey  ^ez 

With  the  first  nine  Pinson  states  and  driving  noises  defined,  the  gyroscope  and 
accelerometer  bias  error  states  and  driving  noises  will  now  be  defined.  First  the 
gyroscope  error  vector  Sg  is  made  up  of  components  Sgx,  Sgy ,  and  Sgz  representing 
the  gyroscope  errors  in  the  body  frame  respectively.  Wg  represents  the  dynamic 
driving  noise  with  body  frame  components  w9x,  w9y,  and  w9z. 


S  g 


r  i 

T 

r 

Sgx  $9y  Sgz 

Wg  = 

_  W9*  W9y  W9z  _ 

Next  the  accelerometers  error  vector  5a  is  made  up  of  components  Sax,  S,ly ,  and 
S0::  representing  acceleration  errors  in  the  body  frame.  W0  represents  the  dynamic 
driving  noise  with  body  frame  components  wax,  way,  and  waz- 


5a  = 


Sar  Sav  Sa , 


W»  = 


w„ 


Wn 


Wn 
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Next,  the  position  states  for  the  image  landmarks  will  be  denoted  as  Tn  with 
NED  components  Tx .  Ty.  and  Tz.  The  targets  are  modeled  as  stationary  with  random 
walk  to  prevent  the  filter  from  converging  [5]  and  [8].  Wtn  represents  the  dynamic 
driving  noise. 

r  -,t  r  -|  t 

Tn  =  Tx  Tz  Tz  Wtn  =  wTx  wTy  wTz 

The  states  are  then  combined  for  each  vehicle  into  the  standard  Pinson  states, 
accelerometer,  and  gyroscope.  For  vehicle  m,  this  would  be  represented  as 

Xm=[fiP  5V  Se  6g  5  a  Tlm  •  •  •  Tn.m  V  (3.1) 

L  mJ 

Note  that  there  are  a  total  of  n  possible  targets. 

The  states  are  further  combined  as  Xm  for  each  vehicle  and  T  y  for  the  target 
for  each  vehicles. 

The  total  state  vector  is  then  formed  by  combining  state  vectors  from  each 
vehicle  (using  a  total  of  M  vehicles). 

XM=[X!  X2  •••  Xm]T  (3.2) 

The  matrix  used  to  describe  the  state  dynamics  or  the  F  matrix  is  the  next 
step  in  the  filter  design.  This  description  will  be  for  individual  vehicle.  The  detailed 
description  of  the  error  dynamics  are  covered  in  Chapter  2.  This  will  be  divided  up 
into  sections  so  that  it  can  more  easily  be  described.  First  are  the  Pinson  states. 
The  full  Pinson  model  is  show  in  Equation  (3.3)  from  [8].  This  also  includes  the 
dynamics  for  the  gyroscope  and  then  the  accelerometer  states. 
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03 

03 

03 

-fl.3 

•  a 
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03 

03 

03 

-I3 

Tb  6  J 

Since  the  landmarks  are 

modeled  as  stationary,  there 

are  no 

dynamics,  and  the 

dynamics  matrix  is  the  zero  matrix. 

F22m  =  [03jvx3jv]  (3-4) 

Where  N  is  the  number  of  landmarks  max. 

With  the  individual  vehicle  state’s  identified,  they  are  then  combined  for  the 
full  F  matrix  for  each  vehicle  as 


F  = 

m. 


Fn 

0 


0 

F22,, 


Where  m  is  the  number  representing  the  vehicle. 
Finally  the  full  F  is  defined  as 


Fi  0  0  0 

0  F2  0  0 

F  = 

0  0  •••  0 

0  0  0  F  m 

Where  M  is  the  total  number  of  vehicles. 


(3.5) 


(3.6) 
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3.3  Measurement  Model 


The  measurement  matrix  for  the  various  types  of  measurements  (radio,  GPS, 
and,  barometer)  can  now  be  defined.  H  is  the  collection  of  the  different  measurements 
that  are  currently  available.  H radiojk  represent  the  measurements  for  the  inter-vehicle 
radio  ranging.  H flVed_radiox  is  the  influence  matrix  for  the  vehicle  to  fixed  radio 
position.  H6ar0i  is  the  influence  matrix  for  the  barometric  measurements.  The  last 
influence  matrix  is  H landmark  f°r  the  images  landmark  measurements. 


H 


radio 


H, 


radiojk 


H, 


radioR 


(3.7) 


H 


fived_radio 


H 


fived_radio 1 


H 


fived_radioT 


(3.8) 


H 


baro 


H, 


baro t 


H, 


baro , 


T 


(3.9) 


H 


landmark 


H, 


landmark . 


H, 


landmark , 


(3.10) 


H 


H 


radio 


H 


fived_radio 


H 


baro 


H 


landmark 


(3.11a) 


To  develop  the  Kalman  filer  to  investigate  the  performance  of  the  ranging 
radios,  the  measurement  model  for  each  of  the  sensors  is  required.  The  measurement 
model  for  the  ranging  radio  between  vehicles  i  and  j  is  the  following 


A  —  XjNSi  +  S'Xi  —  XiNSj  —  dXj 


(3.12) 


Y  =  PiNSi  +  hi  -  vn vs,  -  hi 


(3.13) 
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Z  —  ZlNSi  +  &zi  —  ZrNSj  ~  fizj 


(3.14) 


hradiov  =  Vx 2  +  Y2  +  Z2  (3.15) 

The  measurement  noises  are  independent,  zero-mean,  Gaussian  noises  with 

E[vrvr]  —  cr2  (3.16) 


Xradiojk  h radio 


jk 


+  vr 


(3.17) 


The  perturbation  technique  is  used  to  linearize  h radiojk  about  the  nominal  state 
resulting  in 


H, 


dh 


radio-}- 


radiojk 


<9x 


(3.18) 


The  fixed  radio  is  almost  identical  to  the  inter  vehicle  ranging  case  the  difference 
being  is  that  the  fixed  radio  is  at  a  known  location.  This  changes  h  to 


hfixed_radi0ji  ~  \j ( xINSi  +  $xi  ~~  xl )2  +  ( VlNSi  +  fyi  ~  Ul )2  +  ( zINSi  +  $zi  ~  zlf 

(3.19) 

Where  the  position  of  the  fixed  radio  l  is  given  as  [xi  yi  zi]T . 

The  measurement  noise  is  independent,  zero- mean,  Gaussian  noises  with 

E[vrvr\  =  a2r  (3.20) 
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(3.21) 


E fixed _radic>ji  h fixed_radiOji  T 

The  perturbation  technique  is  used  to  linearize  h fixed_radio:j,  about  the  nominal 
state  resulting  in 


H 


dh 


fixed _radioji 


fixed _radiOji 


dx 


(3.22) 


Now  the  barometric  aid  measurement  will  be  found.  The  barometric  altimeter 
is  modeled  as 


Zbarom  % insm  T  insm  T 

The  statistics  for  the  barometric  measurement  noise  are 


(3.23) 


E[v(,v]  =  cr2Sij  (3.24) 

The  image  measurements  will  now  be  defined  by  closely  following  the  work 
presented  in  [8].  A  monocular  camera  configuration  is  best  for  dismounted  soldiers, 
because  size  and  weight  are  critical  to  combat  operations.  First,  the  landmark’s 
initial  location  estimate  yn  is  found  for  a  feature  of  interest  z  given  the  direction 
associated  with  the  pixel  location  and  the  distance  which  is  measured,  using  a  coded 
aperture. 

The  vector  from  the  camera  to  the  target  is  defined  as 

s'  =  T^z  (3.25) 

The  landmark  location  is  found  by 
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(3.26) 


yn  =  pn  +  c?  [plm+dc^] 

Where  d  represents  the  distance  to  the  landmark  and  is  found  from  the  z 
component  of  yn,  pbcam,  and  sc. 


d 


-  Pn+Cnbpbcam]z 

[cjcjai 


(3.27) 


Now  the  position  of  the  landmark  is  found  and  the  influence  matrix  for  the 
uncertainty  can  be  found  for  the  pixel. 
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zVj 


dz 


H 


zy  ~ 


rppiX 

1  C 


„c  dsc  C 

dsc 

dyn 

Z 

(3.28) 

(3.29) 


and 


dsc 


ccbcbn 


(3.30) 


Hzyj  represents  the  partial  derivatives  with  respect  to  the  pixel  coordinates. 
In  addition  with  range  measurements,  there  is  another  row  in  the  H  matrix  that  is 
equivalent  in  form  to  the  fixed  radio  H  matrix  in  Figure  2.2. 


3-4  Generated  Test  Data 

To  test  whether  imaging  would  assist  the  radio  positioning,  the  following  test 
case  was  developed.  The  scenario  is  for  Stryker  Brigades.  The  terrain  is  a  moderately 
sloping  canyon  with  low  level  vegetation,  and  the  forestation  has  a  medium  canopy. 
There  is  a  road  running  parallel  to  the  bottom  of  the  canyon.  There  are  three  groups 
each  of  which  has  two  dismounted  soldiers.  The  first  group  would  move  down  the 
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Figure  3.1:  This  figure  shows  a  picture  of  the  terrain  in  senario  used  in  the  simulation 


center  of  the  canyon  and  the  other  two  groups  would  flank  the  first  on  the  right  and 
the  left.  The  Stryker  vehicles  would  be  positioned  around  the  soldiers.  The  first  two 
Strykers  would  be  forward  of  the  soldiers  and  at  the  end  of  the  soldier’s  trajectory. 
The  second  two  Strykers  would  start  on  the  road  in  close  proximity  to  where  the 
soldiers  trajectory  starts. 

To  simulate  this  scenario,  the  troops  are  modeled  as  having  a  random  walk  that 
is  constrained  in  heading  to  follow  a  straight  line  trajectory  down  the  valley.  The 
Strykers  are  modeled  as  fixed  radios  with  the  moving  vehicles  positioned  at  a  point 
northwest  of  the  trajectories.  This  would  generate  the  same  geometry  that  would 
be  observed  if  the  moving  vehicles  were  moving  along  the  road  with  the  soldiers. 
Figures  3.1  shows  lightly  sloping  valley  the  terrain  that  the  scenario  is  based  on. 
Figure  ??  shows  the  actual  paths  that  the  troops  are  following  along  the  bottom  of 
the  valley.  It  can  be  see  that  the  soldiers  are  starting  at  about  -100  meters  on  the 
north  axis  and  400  meters  on  the  east  axis  all  in  the  NED  frame.  The  fixed  radios 
are  located  as  seen,  with  heights  of  (  20m,  25m,  20m)  from  left  to  right  in  Figure  ??. 
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Figure  3.2:  This  figure  shows  the  actual  trajectories  of  the  the  four  soldiers  (Red) 
and  the  locations  of  the  fixed  radios  (Blue) 
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Figure  3.3:  This  figure  is  the  acutal  velocity  of  Soldier-1  for  run  1  in  scenario  1 

Figure  3.3  and  3.4  are  the  true  velocity  and  attitude  of  a  soldier  moving  through 
the  scenario. 


Note  the  heading  in  Figure  3.4  is  fluctuating  about  ±57°. 
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Figure  3.4:  This  figure  is  the  Acutal  Attitude  of  Soldier-1  for  run  1  in  Scenario  1 
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IV.  Simulation  and  Results 


To  test  that  the  vision  measurements  will  improve  the  radio  positioning,  the  filter 
developed  in  Chapter  3  was  implemented  in  software.  The  software  simulation  was 
written  in  MATLAB  and  is  based  upon  software  originally  developed  in  [8].  The 
filters  were  run  on  the  scenario  developed  in  Chapter  3.  This  scenario  was  tested  in 
two  cases-one  with  inter-vehicle  radio,  fixed  radio,  and  barometric  measurements; 
and,  one  with  all  the  measurements  from  the  first  case  plus  image  measurements. 
The  simulation  was  run  three  times  each  for  cases  with  two  and  four  soldiers.  A  six 
soldiers  case  was  tried  and  it  was  found  that,  after  approximately  100  seconds,  nu¬ 
merical  precision  issues  began  to  arise.  Due  to  time  constraints  these  issues  were  not 
investigated  further.  Apart  from  the  numerical  precision  issues,  the  six  vehicle  case 
required  17.5  hours  in  MATLAB  2008b  on  a  6600  quad  core  running  approximately 
at  90%  on  all  processors. 

For  each  of  the  scenarios,  plots  of  the  north,  east,  and  down  position  errors  were 
generated  with  a  filter  computed  2  sigma  error.  A  table  was  also  created  with  all  the 
root  mean  square  (RMS)  values  for  each  vehicle  across  time  and  across  all  vehicles 
and  all  time.  Root  mean  square  horizontal  (RMS-H)  values  are  also  generated  for 
each  vehicle  across  time  and  across  all  vehicles  and  all  time. 

During  testing  it  was  found  that  if  the  images  were  incorporated  after  the  radio 
measurements,  the  image  features  became  almost  impossible  to  track,  and  only  one 
or  two  features  could  be  tracked  between  epochs.  With  the  images  incorporated 
before  the  measurements,  feature  tracking  was  much  more  effective.  It  is  believed 
that  this  is  due  to  the  large  variation  in  position  due  to  the  high  radio  standard 
deviation.  In  theory,  the  order  in  which  measurements  are  incorporated  into  the 
filter  should  not  make  any  difference.  However,  since  this  is  an  extended  Kalman 
filter,  relinearization  occurs  after  each  measurement  is  incorporated,  so  it  is  best  to 
incorporate  the  "strongest"  measurements  first. 


4-1 


Table  4.1:  Test  Scenario  1  2:  Radio  Only  case,  Two  Soldiers  case,  NED  frame 


Parameters 

To  (Units) 

Radio  Ranging  (std.) 
Barometer  (std.) 

20  m 

10  m 

Throughout  chapter  4  the  term  ’radio  only’  is  used  when  all  tests  incorporate 
barometric  measurements.  This  nomenclature  is  used  with  the  understanding  that 
barometric  measurements  are  always  used. 

4-1  Test  Scenario  1:  Radio  only,  2  Soldiers 

The  first  test  consisted  of  two  soldiers  with  inter-vehicle  ranging  radios  and 
three  fixed  radio  measurements.  Two  were  stationary  at  a  far  distance  representing 
the  moving  Strykers  and  one  fixed  at  the  end  of  the  soldiers’  trajectory.  Table  4.1 
contains  the  test  parameters  used  in  the  EKF. 

As  seen  in  Figure  4.1,  the  filter  performance  for  Soldier  1  appears  to  be  con¬ 
sistent  with  2-<t  bounds  for  north,  east,  and  down  positions. 

In  Figure  4.2  for  Soldier  1,  the  north,  east,  and  down  plots  were  consistent 
with  2-er  level.  There  seams  to  be  no  issues  regarding  the  velocity  estimates. 

In  Figure  4.3  for  Soldier  1,  the  north  and  east  track  are  consistent  with  2-er 
level.  This  is  not  the  case  for  the  down  axis.  These  start  off  well  and  then  become 
almost  oscillatory.  This  will  show  up  in  all  the  attitude  plots  about  the  down  axis 
for  the  radio  only  cases.  This  is  an  indication  that  heading  could  pose  an  issue  that 
will  need  to  be  addressed.  The  trajectory  that  the  soldiers  follow  has  considerable 
heading  changes  that  could  also  be  adding  to  this  issue.  This  large  heading  change 
phenomena  was  apparent  during  the  generation  feature  as  they  plotted. 

The  plots  of  positions,  velocity,  and  attitude  for  the  other  soldiers  are  very 
similar  and  require  no  further  comment.  All  the  test  scenarios  (1,2,  3  and  4)  plots 
can  however  be  found  the  Appendix. 
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Figure  4.1:  Test  Scenario  1:  Radio  Only- 2  Soldiers  case,  Soldier  1,  Position  Error  in 
NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  4.2:  Test  Scenario  1:  Radio  Only-2  Soldiers  case,  Soldier  1,  Velocity  Error  in 
NED  (Blue),  Filter  Calculated  Error  2-a  (Red),  3  Monte-Carlo  Runs 
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Figure  4.3:  Test  Scenario  1:  Radio  Only-2  Soldiers  case,  Soldier  1,  Attitude  Error  in 
NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Table  4.2:  Test  Scenario  1:  Radio  Only  case,  Two  Soldiers  case,  NED  frame 


RMS-N  (m) 

RMS-E  (m) 

RMS-D  (m) 

RMS-H  (m) 

Run  1  Soldier  1 

8.19 

8.19 

3.11 

11.58 

Run  1  Soldier  2 

9.70 

9.89 

3.67 

13.86 

Run  2  Soldier  1 

7.88 

9.03 

3.53 

11.99 

Run  2  Soldier  2 

9.22 

11.73 

3.95 

14.92 

Run  3  Soldier  1 

8.85 

8.18 

3.33 

12.05 

Run  3  Soldier  2 

9.91 

9.23 

4.07 

13.54 

All  Runs  All  Soldiers 

8.96 

9.37 

3.61 

12.99 

As  can  be  seen  in  Table  4.2  the  results  for  the  soldiers  are  generally  consistent 
between  soldiers. 


4-2  Test  Scenario  2:  Radio  only,  4  Soldiers 

The  second  test  is  for  the  four  soldier  scenario  with  the  same  configurations  as 
the  first  test  scenario. 

Figure  4.4  results  are  similar  to  Scenario  1  with  slightly  higher  errors  in  the 
NED  position.  This  can  be  seen  comparing  Figures  4.1  and  Figure  4.4. 

The  attitude  errors  are  almost  identical  to  the  two  soldier  case  comparing 
Figure  4.2  and  Figure  4.5. 

It  appears  that  adding  more  vehicles  to  the  solution  adds  a  small  amount  of 
uncertainty  to  the  solution.  This  is  evident  in  Table  4.3.  The  results  remain  similar 
between  the  soldiers  which  is  constant  with  Figures  4.1  through  4.5. 

4-3  Test  Scenario  3:  Radio  and  Images,  2  Soldier 

The  third  test  consisted  again  of  two  soldiers  with  inter-vehicle  ranging  radio 
and  three  fixed  radio  measurements,  but  now  the  image  measurements  were  incor¬ 
porated.  As  seen  in  Table  4.4  filter  parameters  are  similar  to  scenario  1  with  the 
imaging  added  for  the  pixels  and  the  monocular  slant  range. 
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Figure  4.4:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  1,  Position  Error  in 
NED  (Blue),  Filter  Calculated  Error  2-er  (Red),  3  Monte-Carlo  Runs 


Table  4.3:  Test  Scenario  2:  Radio  Only  case,  Four  Soldiers  case,  NED  frame 


RMS-N  (m) 

RMS-E  (m) 

RMS-D  (m) 

RMS-H  (m) 

Run  1  Soldier  1 

10.93 

16.64 

3.42 

19.91 

Run  1  Soldier  2 

11.08 

16.85 

3.94 

20.17 

Run  1  Soldier  3 

14.84 

23.80 

3.73 

28.04 

Run  1  Soldier  4 

16.47 

29.81 

4.04 

34.06 

Run  2  Soldier  1 

9.04 

9.70 

3.34 

13.26 

Run  2  Soldier  2 

10.48 

12.40 

3.53 

16.45 

Run  2  Soldier  3 

12.87 

20.39 

4.56 

24.11 

Run  2  Soldier  4 

11.99 

13.99 

3.59 

18.42 

Run  3  Soldier  1 

9.78 

11.98 

3.44 

15.46 

Run  3  Soldier  2 

9.97 

14.50 

3.76 

17.59 

Run  3  Soldier  3 

16.71 

23.81 

4.57 

29.09 

Run  3  Soldier  4 

9.85 

15.47 

3.88 

18.34 

All  Runs  All  Soldiers 

12.00 

17.45 

3.82 

21.24 
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Figure  4.5:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  1,  Attitude  Error  in 
NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 


Table  4.4:  Test  Scenarios  3  4:  Radio  and  Image  case,  NED  frame 


Parameters 

1  -a  (Units) 

Radio  Ranging  (std.) 

20  (m) 

Barometer  (std.) 

10  (m) 

Image  (std.) 

1  (pixel) 

Image  slant  range  (std.) 

5  (m) 
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Figure  4.6:  Test  Scenario  3:  Radio  and  Image-2  Soldiers  case,  Soldier  1,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-er  (Red),  3  Monte-Carlo  Runs 

As  seen  in  Figure  4.6  there  is  immediately  a  difference  in  scale.  The  errors  are 
reduced  significantly  from  the  radio  only  case  (Figure  4.1).  While  the  covariance  is 
around  1  m  the  error  grows  up  to  around  5  m  for  north  and  east.  This  is  better  than 
the  20  m  that  was  found  in  Figure  4.1,  but  not  consistent  with  the  covariance  that 
is  created  by  the  filter.  Further  tuning  of  the  initial  covariance  could  help  correct 
this.  There  was  one  area  in  the  MATLAB  code  that  didn’t  have  a  baseline  to  verify 
performance  gains  and  that  was  the  monocular  slant  range.  By  testing  the  code 
without  the  slant  range  it  was  found  that  there  is  a  problem  within  the  code  for 
the  slant  range  as  seen  in  Figure  4.7  .  Due  to  time  constraints  this  will  have  to  be 
explored  by  further  research. 

The  image  measurement  definitely  aided  the  attitude  error.  As  seen  by  com¬ 
paring  the  Radio  Only  case  (Figure  4.3)  with  the  Radio  and  Image  case  (Figure  4.8), 
the  images  improved  the  attitude  errors  by  more  than  an  order  of  magnitude.  This 
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Figure  4.7:  Test  Scenario  3  Special:  Radio  and  Image-2  Soldiers  case,  Soldier  1, 
Position  Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  1  Run  100  seconds 
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Figure  4.8:  Test  Scenario  3:  Radio  and  Image-2  Soldiers  case,  Soldier  1,  Attitude 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-er  (Red),  3  Monte-Carlo  Runs 

finding  adds  to  the  importance  of  incorporating  the  image  measurement  before  the 
radios. 

The  velocity  plot  is  in  the  Appendix  but  doesn’t  show  any  beneficial  infor¬ 
mation,  however,  the  RMS  values  do.  As  seen  in  Table  4.5,  Test  Scenario  3  Radio 
only-2  Soldiers,  the  values  are  much  smaller  than  those  found  in  Test  Scenario  3 
Radio  only-2  Soldiers  of  Table  4.2.  It  would  appear  that  the  slant  range  is  truly 
effecting  run  l’s  of  Soldier  one’s.  Comparing  Table  4.5  and  Table  4.2  there  appears 
to  be  an  improvement  in  the  RMS  error  by  a  factor  of  approximately  2. 

4-4  Test  Scenario  4:  Radio  and  Images,  4  Soldiers 

The  fourth  test  is  again  for  the  four  soldier  scenario  with  the  same  configura¬ 
tions  as  scenario  2  with  image  measurements  incorporated.  The  results  are  shown 
in  Figures  4.9-4.11. 
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Table  4.5:  Test  Scenario  3:  Radio  and  Image  case,  Two  Soldiers  case,  NED  frame 


RMS-N  (m) 

RMS-E  (m) 

RMS-D  (m) 

RMS-H  (m) 

Run  1  Soldier  1 

1.24 

1.07 

0.31 

1.64 

Run  1  Soldier  2 

2.81 

2.68 

0.20 

3.88 

Run  2  Soldier  1 

3.30 

2.83 

0.39 

4.35 

Run  2  Soldier  2 

6.76 

6.17 

0.31 

9.15 

Run  3  Soldier  1 

2.04 

2.21 

0.48 

3.01 

Run  3  Soldier  2 

9.08 

8.54 

0.44 

12.47 

All  Runs  All  Soldiers 

4.21 

3.92 

0.36 

5.75 

For  Figure  4.9  vehicle  four  has  the  drift  problem  as  seen  in  scenario  3  for 
two  vehicles  with  imaging  and  radio  measurements.  The  slant  range  appears  to  be 
influencing  it  and  overly  improving  the  covariance. 

Additional  to  the  slant  range  effects  in  Figure  4.9  there  are  other  effects  from 
what  appears  to  be  heading  errors  allowing  the  solder  to  drift  off  as  seen  in  Figure 
4.10. 

Figure  4.11  shows  even  better  attitude  correction,  save  one  case  that  showed  a 
biased  error  immediately.  This  again  shows  image  adding  improves  the  radio  ranging 
in  the  case  where  the  initial  first  60  second  of  heading  measurements  are  reasonably 
good. 

The  runs  with  heading  errors  starting  at  the  beginning  of  the  run  have  the 
highest  RMS  values.  The  data  without  large  heading  errors  was  slightly  improved 
over  scenario  three.  This  can  be  seen  by  comparing  Table  4.5  and  Table  4.6.  So, 
if  image  features  are  tracked  reasonably  well  at  the  beginning  of  the  run,  the  filter 
has  a  much  better  performance  over  the  radio  only  cases.  It  is  possible  that  further 
tuning  of  the  filter  would  improve  the  ability  of  the  filter  to  initially  converge  onto 
the  correct  attitude. 

From  reviewing  the  data,  there  are  areas  that  need  to  be  addressed  in  further 
research,  like  the  image’s  slant  range  measurement  and  the  heading  errors.  Over  all, 
the  system  shows  great  promise  for  improving  performance. 
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Figure  4.9:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  4,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 


Table  4.6:  Test  Scenario  4:  Radio  and  Image  case,  Four  Soldiers  case,  NED  frame 


RMS-N  (m) 

RMS-E  (m) 

RMS-D  (m) 

RMS-H  (m) 

Run  1  Soldier  1 

4.88 

4.65 

0.41 

6.73 

Run  2  Soldier  1 

0.63 

0.65 

0.22 

0.91 

Run  3  Soldier  1 

6.46 

6.41 

0.42 

9.11 

Run  1  Soldier  2 

11.83 

11.31 

1.01 

16.37 

Run  2  Soldier  2 

8.69 

8.20 

0.21 

11.95 

Run  3  Soldier  2 

3.54 

3.95 

0.39 

5.30 

Run  1  Soldier  3 

0.94 

1.17 

0.24 

1.50 

Run  2  Soldier  3 

0.54 

0.52 

0.26 

0.75 

Run  3  Soldier  3 

0.92 

1.05 

0.32 

1.40 

Run  1  Soldier  4 

6.90 

6.13 

0.48 

9.22 

Run  2  Soldier  4 

5.97 

5.89 

0.50 

8.39 

Run  3  Soldier  4 

2.03 

1.55 

0.29 

2.55 

All  Runs  All  Soldiers 

4.44 

4.29 

0.40 

6.18 
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Figure  4.10:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  1,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  4.11:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  4,  Attitude 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-a  (Red),  3  Monte-Carlo  Runs 
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V.  Conclusion  and  Recommendations 


5. 1  Conclusion 

The  results  from  the  simulations  were  promising  looking  at  Table  5.1.  There 
is  an  improvement  in  the  results  and  image  measurements  improve  the  capabilities 
of  the  radio  ranging  system.  The  improvement  on  all  axis  were  on  the  order  of  ap¬ 
proximately  a  factor  of  2.  There  is  good  support  that  slant  range  measurements  are 
causing  issues,  especially  with  respect  to  the  slow  drift  of  the  error  as  seen  in  the 
position  plots  with  imaging  used.  A  test  case  with  slant  range  only  proved  this  as 
it  didn’t  have  a  drift.  An  additional  issue  can  also  occur  if  the  image  measurements 
can’t  converge  in  the  first  minute  of  operation.  Looking  at  larger  Monte  Carlo  simu¬ 
lations  of  the  simulation  will  help  to  verify  how  often  the  imaging  can  possibly  have 
this  error.  Looking  over  Table  5.1  it  was  also  discovered  that  as  the  number  of  sol¬ 
diers  increased  there  was  an  increase  in  the  RMS  error  of  a  factor  of  approximately 
1.3  for  Scenarios  1  and  2.  For  Scenarios  3  and  4  there  was  an  increase  in  RMS  error 
again  but  it  was  only  a  factor  of  1.08.  It  is  possible  that  adding  more  vehicles  acts 
like  adding  more  process  noise  to  the  system  as  more  vehicles  are  added  but  with 
imaging  this  is  constrained 

The  centralized  approach  presented  in  this  thesis  forms  a  solid  base  for  moving 
this  research  into  distributed  filter  structures,  such  as  [1]. 

As  mentioned  in  Chapter  4,  the  order  in  which  the  measurements  are  incor¬ 
porated  is  important  to  the  filter.  If  a  large  noise  measurement  is  incorporated  into 


Table  5.1:  Test  Senario  1-4  Both  cases  for  All  Soldiers 


RMS-N  (m) 

RMS-E  (m) 

RMS-D  (m) 

RMS-H  (m) 

Test  Scenario  1:  Radio  Only-2  soldiers  case,  All  Soldier 

8.96 

9.37 

3.61 

12.99 

Test  Scenario  2:  Radio  Only-4  soldiers  case,  All  Soldier 

12.00 

17.45 

3.82 

21.24 

Test  Scenario3:  Radio  and  Image-2  soldiers  case,  All  Soldier 

4.21 

3.92 

0.36 

5.75 

Test  Scenario  4:  Radio  and  Image-4  soldiers  case,  All  Soldier 

4.44 

4.29 

0.40 

6.18 
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the  position  before  the  image  measurement  it  can  adversely  effect  the  filters  ability 
to  track  image  features. 

Bookkeeping  techniques  like  the  "Rosetta  Stone"  were  crucial  to  tracking  the 
many  states  that  the  filter  had  in  its  4  and  6  soldier  cases.  Without  such  techniques 
the  ability  to  delineate  the  different  states  of  the  soldiers  and  targets  would  have  been 
more  complicated  and  possibly  less  efficient  at  generating  the  covariance  matrix  P 
and  the  influence  matrix  H. 

5.2  Recommendations 

With  the  initial  simulations  conducted,  further  tests  should  be  made  to  see 
how  the  system  will  perform  in  other  scenarios.  As  was  already  seen,  numerical 
issues  started  to  arise  when  six  vehicles  were  simulated.  There  is  also  the  possibility 
that  the  use  of  a  partial  filter  could  improve  the  estimates  if  there  are  any  issues  in 
linearization. 

There  are  additional  scenarios  that  could  also  be  tested.  Different  conditions 
such  as  urban,  large  mountains  and  thicker  foliage  could  be  modeled.  Scenarios  from 
the  troops  in  the  field  would  be  the  best  way  to  test  real  world  scenarios.  Once  these 
have  been  simulated,  field  tests  can  be  accomplished  to  determine  a  rigorous  model 
of  the  radios  to  include  issue  like  multipath  and  tropospheric  delays.  The  work  in 
single  camera  ranging  should  also  be  incorporated  to  make  the  monocular  camera 
slant  range  measurement  error  model  more  precise. 

The  ability  to  match  features  between  the  different  soldiers  would  be  another 
step  that  could  even  further  reduce  the  attitude  errors  that  were  seen  by  correlating 
the  vehicle’s  heading. 

The  addition  of  other  measurements  could  also  continue  to  improve  the  esti¬ 
mate,  like  from  one  GPS  satellite  or  some  other  signal. 
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VI.  Appendix.  Complete  Collection  Of  Plots  For  Chapter  Four 


Test  Scenario  2:  Radio  only,  2  Soldiers 


Figure  6.1:  Test  Scenario  1:  Radio  Only-2  Soldiers  case,  Soldier  1,  Position  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.2:  Test  Scenario  1:  Radio  Only-2  Soldiers  case,  Soldier  1,  Velocity  Error 
in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.3:  Test  Scenario  1:  Radio  Only-2  Soldiers  case,  Soldier  1,  Attitude  Error 
in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 


Figure  6.4:  Test  Scenario  1:  Radio  Only-2  Soldiers  case,  Soldier  2,  Position  Error 
in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.5:  Test  Scenario  1:  Radio  Only-2  Soldiers  case,  Soldier  2,  Velocity  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 


Figure  6.6:  Test  Scenario  1:  Radio  Only-2  Soldiers  case,  Soldier  2,  Attitude  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 

Test  Scenario  2:  Radio  only,  4  Soldiers 
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Figure  6.7:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  1,  Position  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.8:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  1,  Velocity  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.9:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  1,  Attitude  error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.10:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  2,  Position  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.11:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  2,  Velocity  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-er  (Red),  3  Monte-Carlo  Runs 
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Figure  6.12:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  2,  Attitude  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-er  (Red),  3  Monte-Carlo  Runs 
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Figure  6.13:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  3,  Position  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 


Figure  6.14:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  3,  Velocity  Error 
in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.15:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  3,  Attitude  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 


Figure  6.16:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  4,  Position  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.17:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  4,  Velocity  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-er  (Red),  3  Monte-Carlo  Runs 


Figure  6.18:  Test  Scenario  2:  Radio  Only-4  Soldiers  case,  Soldier  4,  Attitude  Error 
in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 

Test  Scenario  3:  Radio  and  Images,  2  Soldiers 
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Figure  6.19:  Test  Scenario  3:  Radio  and  Image-2  Soldiers  case,  Soldier  1,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.20:  Test  Scenario  3:  Radio  and  Image-2  Soldiers  case,  Soldier  1,  Velocity 
error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.21:  Test  Scenario  3:  Radio  and  Image-2  Soldiers  case,  Soldier  1,  Attitude 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.22:  Test  Scenario  3:  Radio  and  Image-2  Soldiers  case,  Soldier  2,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.23:  Test  Scenario  3:  Radio  and  Image-2  Soldiers  case,  Soldier  2,  Velocity 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.24:  Test  Scenario  3:  Radio  and  Image-2  Soldiers  case,  Soldier  2,  Attitude 
error  in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 

Test  Scenario  4:  Radio  and  Images,  4  Soldiers 
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Figure  6.25:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  1,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.26:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  1,  Velocity 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 


6-13 


0 


400  500  600 

Time  (s)  Soldier  1 


Figure  6.27:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  1,  Attitude 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-rr  (Red),  3  Monte-Carlo  Runs 


Figure  6.28:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  2,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.29:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  2,  Velocity 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.30:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  2,  Attitude 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-cr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.31:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  3,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.32:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  3,  Velocity 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-rr  (Red),  3  Monte-Carlo  Runs 
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Figure  6.33:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  3,  Attitude 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.34:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  4,  Position 
Error  in  NED  (Blue),  Filter  Calculated  Error  2-a  (Red),  3  Monte-Carlo  Runs 
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Figure  6.35:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  4,  Velocity 
error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 


Figure  6.36:  Test  Scenario  4:  Radio  and  Image-4  Soldiers  case,  Soldier  4,  Attitude 
Error  in  NED  (Blue),  Filter  Calculated  Error  2 -a  (Red),  3  Monte-Carlo  Runs 
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